#include "PostProcessor.hpp"

namespace visionhw
{

    PostProcessor::PostProcessor()
        : _cloud_in(new CloudType),
          _indices_src_rmnan(),
          _voxel(),
          _plane_coefficients(new pcl::ModelCoefficients),
          _plane_inliers(new pcl::PointIndices),
          _plane_seg(),
          _extractor(),
          _cluster_tree(new pcl::search::KdTree<PointType>),
          _cluster_indices(),
          _ec(),
          _tmpp(new pcl::PointIndices),
          _normal(),
          _normal_tree(),
          _cloud_normal(new pcl::PointCloud<pcl::Normal>),
          _fpfh(),
          _fpfh_tree(new pcl::search::KdTree<PointType>),
          _fpfh_cloud(new pcl::PointCloud<pcl::FPFHSignature33>())
    {
        init();
    }

    void PostProcessor::init()
    {
        // 下采样
        _voxel.setLeafSize(0.005f, 0.005f, 0.005f);

        // 去平面
        _plane_seg.setOptimizeCoefficients(true);
        _plane_seg.setModelType(pcl::SACMODEL_PLANE); //设置模型类型
        _plane_seg.setMethodType(pcl::SAC_RANSAC);    //设置随机采样一致性方法类型
        _plane_seg.setMaxIterations(10000);           //设置最大迭代次数
        _plane_seg.setDistanceThreshold(0.01);        //设定距离阀值，距离阀值决定了点被认为是局内点是必须满足的条件
        _extractor.setNegative(true); // 滤除平面

        // 聚类
        _ec.setClusterTolerance(0.02); //设置近邻搜索的搜索半径为2cm
        _ec.setMinClusterSize(300);    //设置一个聚类需要的最少点数目为100
        _ec.setMaxClusterSize(20000);  //设置一个聚类需要的最大点数目为25000

        // 法线计算
        _normal.setRadiusSearch(0.02);
        
        // FPFH
        _fpfh.setRadiusSearch(0.05);
    }

    void PostProcessor::setInputCloud(const CloudType::Ptr cloud_input)
    {
        *_cloud_in = *cloud_input; // 复制
    }
    void PostProcessor::process(CloudType &cloud_output, pcl::PointCloud<pcl::FPFHSignature33> &fpfh_cloud)
    {
        pcl::removeNaNFromPointCloud(*_cloud_in, *_cloud_in, _indices_src_rmnan);

        // 下采样滤波
        _voxel.setInputCloud(_cloud_in);
        _voxel.filter(*_cloud_in);

        if (_saveIntermediatePly)
            pcl::io::savePLYFile("outputply/下采样滤波后.ply", *_cloud_in);

        // 去平面
        if (!_is_ply)
        {
            _plane_seg.setInputCloud(_cloud_in);
            _plane_seg.segment(*_plane_inliers, *_plane_coefficients);

            // 滤除平面取出非平面
            _extractor.setInputCloud(_cloud_in);
            _extractor.setIndices(_plane_inliers);
            _extractor.filter(*_cloud_in);

            if (_saveIntermediatePly)
                pcl::io::savePLYFile("outputply/去平面后.ply", *_cloud_in);

            // 聚类
            _cluster_tree->setInputCloud(_cloud_in);
            _ec.setSearchMethod(_cluster_tree); //设置点云的搜索机制
            _ec.setInputCloud(_cloud_in);

            //从点云中提取聚类，并将点云索引保存在cluster_indices中
            _ec.extract(_cluster_indices); 
            
            *_tmpp = _cluster_indices[0];
            _extractor.setIndices(_tmpp);
            _extractor.setNegative(false);
            _extractor.filter(*_cloud_in);

            if (_saveIntermediatePly)
                pcl::io::savePLYFile("outputply/聚类后.ply", *_cloud_in);
        }

        // 法线计算
        _normal.setInputCloud(_cloud_in);
        _normal.setSearchMethod(_normal_tree);

        _normal.compute(*_cloud_normal);

        // FPFH
        _fpfh.setInputCloud(_cloud_in);
        _fpfh.setInputNormals(_cloud_normal);
        _fpfh.setSearchMethod(_fpfh_tree);

        _fpfh.compute(*_fpfh_cloud);

        // output
        cloud_output = *_cloud_in;
        fpfh_cloud = *_fpfh_cloud;
    }

}